#pragma once
#include <math.h>
#include <d3d9.h>
#include <intrin.h>

/**
 * Klasa wektora 3D
 *
 * @author Krzysztof Klein
 * @version 0.1
 */
class Vector
{

	/**
	 * Wspolrzedne wektora
	 */
	__m128 vec;

public:

	/**
	 * Konstruktor domyslny
	 */
	inline Vector() { vec = _mm_setzero_ps(); }

	/**
	 * Konstruktor z spakowana liczba SSE
	 */
	inline Vector(__m128 vec) : vec(vec) { }

	/**
	 * Konstruktor z wspolrzednych kartezjanskich
	 */
	inline Vector(float x, float y, float z) { vec = _mm_set_ps(x, y, z, 0.0f); }

//#ifdef D3DVECTOR_DEFINED
	/**
	 * Konstruktor z wektora Direct3D
	 */
	inline Vector(const D3DVECTOR &v) { vec = _mm_set_ps(v.x, v.y, v.z, 0.0f); }
//#endif

	// @{
	/**
	 * Operator arytmetyczny
	 */
	inline Vector operator+(const Vector &v) const { return Vector(_mm_add_ps(vec, v.vec)); }
	inline Vector& operator+=(const Vector &v) { vec = _mm_add_ps(vec, v.vec); return *this; }
	inline Vector operator-(const Vector &v) const { return Vector(_mm_sub_ps(vec, v.vec)); }
	inline Vector& operator-=(const Vector &v) { vec = _mm_sub_ps(vec, v.vec); return *this; }
	inline Vector operator*(const Vector &v) const { return Vector(_mm_mul_ps(vec, v.vec)); }
	inline Vector& operator*=(const Vector &v) { vec = _mm_mul_ps(vec, v.vec); return *this; }
	inline Vector operator/(const Vector &v) const { return Vector(_mm_div_ps(vec, v.vec)); }
	inline Vector& operator/=(const Vector &v) { vec = _mm_div_ps(vec, v.vec); return *this; }
	inline Vector operator*(float t) const { return Vector(_mm_mul_ps(vec, _mm_set_ps(t, t, t, t))); }
	inline Vector& operator*=(float t) { vec = _mm_mul_ps(vec, _mm_set_ps(t, t, t, t)); return *this; }
	inline Vector operator/(float t) const { return Vector(_mm_div_ps(vec, _mm_set_ps(t, t, t, t))); }
	inline Vector& operator/=(float t) { vec = _mm_div_ps(vec, _mm_set_ps(t, t, t, t)); return *this; }
	// @}


	// @{
	/**
	 * Operator porownania
	 */
	inline bool operator<=(const Vector &v) const { return 0x0F==_mm_movemask_ps(_mm_cmple_ps(vec, v.vec)); }
	inline bool operator<(const Vector &v) const { return 0x0F==_mm_movemask_ps(_mm_cmpnge_ps(vec, v.vec)); }
	inline bool operator>=(const Vector &v) const { return 0x0F==_mm_movemask_ps(_mm_cmpge_ps(vec, v.vec)); }
	inline bool operator>(const Vector &v) const { return 0x0F==_mm_movemask_ps(_mm_cmpnle_ps(vec, v.vec)); }
	// @}


	/**
	 * Normalizuje wektor
	 *
	 * @return Znormalizowany wektor
	 */
	inline Vector& normalize() {*this *= getLengthReciprocal(); return *this; }
	
	
	/**
	 * Oblicza iloczyn skalarny
	 *
	 * @param v Wektor
	 * @return Iloczyn skalarny
	 */
	inline float dotProduct(const Vector &v) const
	{
		// iloczyn skalarny jest szybszy na FPU
		return getX()*v.getX() + getY()*v.getY() + getZ()*v.getZ();
	}



	/**
	 * Oblicza iloczyn wektorowy
	 *
	 * @param v Wektor
	 * @return Iloczyn wektorowy
	 */
	inline Vector crossProduct(const Vector &v) const
	{
		__m128 tmp1, tmp2, tmp3;
		tmp1 = _mm_shuffle_ps(vec, vec, 0xD8);
		tmp2 = _mm_shuffle_ps(v.vec, v.vec, 0xE1);
		tmp3 = _mm_mul_ps(tmp1, tmp2);
		tmp1 = _mm_shuffle_ps(vec, vec, 0xE1);
		tmp2 = _mm_shuffle_ps(v.vec, v.vec, 0xD8);
		tmp3 = _mm_sub_ps(tmp3, _mm_mul_ps(tmp1, tmp2));
		return Vector(tmp3);
	}


	/**
	 * Oblicza kwadrat dlugosci wektora
	 *
	 * @return Kwadrat dlugosci wektora
	 */
	inline float getLengthSquared() const { return dotProduct(*this); }
	
	
	/**
	 * Oblicza dlugosc wektora
	 *
	 * @return Dlugosc wektora
	 */
	inline float getLength() const { return sqrt(getLengthSquared());}
	
	
	/**
	 * Oblicza odwrotnosc dlugosci wektora
	 *
	 * @return Odwrotnosc dlugosci wektora
	 */
	inline float getLengthReciprocal() const
	{
		float length, lengthSquared = getLengthSquared();
		_mm_store_ss(&length, _mm_rsqrt_ss(_mm_load_ss(&lengthSquared)));
		return length;
	}


	/**
	 * Ustawia dlugosc wektora.
	 * Kierunek wektora pozostaje bez zmian.
	 *
	 * @return Dlugosc wektora
	 */
	inline void setLength(float newLength) { normalize(); *this *= newLength; }


	/**
	 * Zwraca wektor jako tablice 3 liczb (wspolrzednych kartezjanskich)
	 *
	 * @return Tablica wspolrzednych
	 */
	inline operator const float*() const { return &(vec.m128_f32[0]); }


	// @{
	/**
	 * Pobiera wspolrzedna
	 */
	inline float getX() const { return vec.m128_f32[3]; }
	inline float getY() const { return vec.m128_f32[2]; }
	inline float getZ() const { return vec.m128_f32[1]; }
	// @}


	// @{
	/**
	 * Ustawia wspolrzedna
	 */
	inline void setX(float X) { vec.m128_f32[3] = X; }
	inline void setY(float Y) { vec.m128_f32[2] = Y; }
	inline void setZ(float Z) { vec.m128_f32[1] = Z; }
	// @}

};
